// motoin9_dll.cpp : 定义 DLL 应用程序的导出函数。
//
#include "motion9_dll.h"
#include "./kinematics/kinematics.h"
#include "./trajectory_plane/fr.h"
#include "./trajectory_plane/para_check.h"
#include "./trajectory_plane/tcq.h"
#include "./trajectory_plane/tp.h"
#include "c_mutex.h"
#include "control.h"
#include "msg_queue.h"
#include "position_out.h"
#include "process_inputs.h"
#include <stdarg.h>
#include <stdio.h>
#include <sys/time.h>
#include <sys/times.h>
#include <time.h>
#include "joint_controller/joint_queue.h"
//
#define SERVICE_MUTEX_OUT_TIME 20 //20ms

/*
全局变量
*/
char CTRL_VERSION[32] = CVERSION;
STRUCT_CONTROL Ct = {0};

//************************************
// Method:    Motion_Init
// FullName:  Motion_Init
// Access:
// Returns:   MOTOIN9_DLL_API CALL int
// Qualifier:
// Parameter: STRUCT_MOTION_PARM init_param
//************************************
MOTOIN9_DLL_API CALL int Motion_Init(STRUCT_MOTION_PARM init_param) {
    int i = 0;
    if (1 == Ct.init_done) {
        return 0;
    }
    DLog("Motion version = %s", CTRL_VERSION);
    msqInit();
    Ct.conf = init_param;
    if (Ct.conf.callback_msg == NULL) return -1;
    if (Ct.conf.callback_out == NULL) return -1;
    //calc period
    Ct.conf.inter_period_recip = (float) 1.0 / Ct.conf.inter_period;
    Ct.conf.inter_period_cubic = Ct.conf.inter_period_recip * Ct.conf.inter_period_recip;
    //set joint enable
    if (SCARA == Ct.conf.machine_struct) {
        //enable joint 0 to 3
        memset(Ct.conf.joint_enable, 0x01, 4);
        // Kinematic_SetMachinePara(Ct.conf.dh_machine, Ct.conf.dh_tool);
    }
    //set velocity rate
    for (i = 0; i < MAX_JOINT; i++) {
        if (1 == Ct.conf.joint_enable[i]) {
            //??? if vel_max[0] == 0 || acc_max[0] == 0
            if (Ct.conf.vel_joint.vel_max[i] < TP_VEL_EPSILON) return -1;
            if (Ct.conf.vel_joint.acc_max[i] < TP_ACCEL_EPSILON) return -1;
            Ct.joint_max_vel_rate[i] = Ct.conf.vel_joint.vel_max[0] / Ct.conf.vel_joint.vel_max[i];
            Ct.joint_max_acc_rate[i] = Ct.conf.vel_joint.acc_max[0] / Ct.conf.vel_joint.acc_max[i];
        } else {
            Ct.joint_max_vel_rate[i] = 0;
            Ct.joint_max_acc_rate[i] = 0;
        }
        Ct.conf.vel_joint.vel_start[i] = 0;
        Ct.conf.vel_joint.vel_stop[i] = 0;
    }
    /*set line and rot velocity*/
    Ct.conf.vel_line.vel_start = 0;
    Ct.conf.vel_line.vel_stop = 0;
    Ct.conf.vel_rot.vel_start = 0;
    Ct.conf.vel_rot.vel_stop = 0;
    Ct.init_done = 1;
    mtInterfaceCreat();
    // Kinematic_SetMachinePara(Ct.conf.dh_machine, Ct.conf.dh_tool);
    Controller_Initial(); //配置成功
    DLog("Tro->--------------------------------------");
    /*initial already done*/
    Controller_MsgVp(MOTION_MSG_INIT_DONE, "%s", CTRL_VERSION);
    return 0;
}

MOTOIN9_DLL_API CALL int Motion_SetTool(const STRUCT_POSE *tool) {
    int ret = 0;
    mtInterfaceLock(LOCKER_SERVICE, SERVICE_MUTEX_OUT_TIME);
    if (Motion_QueryMoveDone() == 0) {
        STRUCT_TOOL_DESCRIPTION td;
        td.x = tool->x;
        td.y = tool->y;
        td.z = tool->z;
        td.rx = tool->rx;
        td.ry = tool->ry;
        td.rz = tool->rz;
        Kinematic_SetTool(td);
        ret = 0;
    } else {
        ret = -1;
    }
    mtInterfaceUnlock(LOCKER_SERVICE);
    return ret;
}

/*Set current posit*/
MOTOIN9_DLL_API CALL int Motion_SetCurrentPosition(double curr_posit[MAX_JOINT]) {
    mtInterfaceLock(LOCKER_SERVICE, SERVICE_MUTEX_OUT_TIME);
    if (Motion_QueryMoveDone() == 0) {
        memcpy(Ct.curr_posit, curr_posit, sizeof(double) * MAX_JOINT);
        memcpy(Ct.tp.crp, Ct.curr_posit, sizeof(double) * MAX_JOINT);
        memcpy(Ct.tp.goal_posit, Ct.curr_posit, sizeof(double) * MAX_JOINT);
        memcpy(Ct.fr.crp, Ct.curr_posit, sizeof(double) * MAX_JOINT);
    } else {
        Controller_MsgVp(MOTION_WARN_SET_POSIT_WHILE_MOVING, "can't set current posit while move\n");
    }
    mtInterfaceUnlock(LOCKER_SERVICE);
    DLog("Tro->Set Current Position");
    return 0;
}

/*
---------------Get Input Command---------------
*/
//启动PLANE模式
MOTOIN9_DLL_API CALL int Motion_CommandPlane() {
    DLog("Tro->Get Command->PALNE");
    mtInterfaceLock(LOCKER_SERVICE, SERVICE_MUTEX_OUT_TIME);
    if (Controller_CheckInitialDone() != 0) {
        mtInterfaceUnlock(LOCKER_SERVICE);
        return -1;
    }
    if (Controller_CheckEmergency() != 0) {
        mtInterfaceUnlock(LOCKER_SERVICE);
        return -1;
    }
    if (Controller_CheckPause() != 0) {
        mtInterfaceUnlock(LOCKER_SERVICE);
        return -1;
    }
    if (Controller_CheckAbort() != 0) {
        mtInterfaceUnlock(LOCKER_SERVICE);
        return -1;
    }
    int ret = Cmd_ParseCommand_CMD_PLANE();
    mtInterfaceUnlock(LOCKER_SERVICE);
    return ret;
}
//启动自由模式
MOTOIN9_DLL_API CALL int Motion_CommandFree() {
    DLog("Tro->Get Command->Free");
    mtInterfaceLock(LOCKER_SERVICE, SERVICE_MUTEX_OUT_TIME);
    if (Controller_CheckInitialDone() != 0) {
        mtInterfaceUnlock(LOCKER_SERVICE);
        return -1;
    }
    if (Controller_CheckEmergency() != 0) {
        mtInterfaceUnlock(LOCKER_SERVICE);
        return -1;
    }
    if (Controller_CheckPause() != 0) {
        mtInterfaceUnlock(LOCKER_SERVICE);
        return -1;
    }
    if (Controller_CheckAbort() != 0) {
        mtInterfaceUnlock(LOCKER_SERVICE);
        return -1;
    }
    int ret = Cmd_ParseCommand_CMD_FREE();
    mtInterfaceUnlock(LOCKER_SERVICE);
    return ret;
}
//急停模式
MOTOIN9_DLL_API CALL int Motion_CommandEmergency() {
    DLog("Tro->Get Command->Emergency");
    mtInterfaceLock(LOCKER_SERVICE, SERVICE_MUTEX_OUT_TIME);
    if (Controller_CheckInitialDone() != 0) {
        mtInterfaceUnlock(LOCKER_SERVICE);
        return -1;
    }
    if (Controller_CheckEmergency() != 0) {
        mtInterfaceUnlock(LOCKER_SERVICE);
        return -1;
    }
    int ret = Cmd_ParseCommand_CMD_EMERGEMCY();
    mtInterfaceUnlock(LOCKER_SERVICE);
    return ret;
}
//解除急停
MOTOIN9_DLL_API CALL int Motion_CommandEmergencyRelease() {
    DLog("Tro->Get Command->EmergencyRelease");
    mtInterfaceLock(LOCKER_SERVICE, SERVICE_MUTEX_OUT_TIME);
    if (Controller_CheckInitialDone() != 0) {
        mtInterfaceUnlock(LOCKER_SERVICE);
        return -1;
    }
    int ret = Cmd_ParseCommand_CMD_EMERGEMCY_RELEASE();
    mtInterfaceUnlock(LOCKER_SERVICE);
    return ret;
}
//停止
MOTOIN9_DLL_API CALL int Motion_CommandAbort() {
    DLog("Tro->Get Command->Abort");
    mtInterfaceLock(LOCKER_SERVICE, SERVICE_MUTEX_OUT_TIME);
    if (Controller_CheckInitialDone() != 0) {
        mtInterfaceUnlock(LOCKER_SERVICE);
        return -1;
    }
    if (Controller_CheckEmergency() != 0) {
        mtInterfaceUnlock(LOCKER_SERVICE);
        return -1;
    }
    //if (Controller_CheckAbort() != 0){ mtInterfaceUnlock(LOCKER_SERVICE); return -1; }  //无需处理
    int ret = Cmd_ParseCommand_CMD_ABORT();
    mtInterfaceUnlock(LOCKER_SERVICE);
    return ret;
}
//暂停
MOTOIN9_DLL_API CALL int Motion_CommandPause() {
    DLog("Tro->Get Command->Pause");
    mtInterfaceLock(LOCKER_SERVICE, SERVICE_MUTEX_OUT_TIME);
    if (Controller_CheckInitialDone() != 0) {
        mtInterfaceUnlock(LOCKER_SERVICE);
        return -1;
    }
    if (Controller_CheckEmergency() != 0) {
        mtInterfaceUnlock(LOCKER_SERVICE);
        return -1;
    }
    if (Controller_CheckAbort() != 0) {
        mtInterfaceUnlock(LOCKER_SERVICE);
        return -1;
    }
    int ret = Cmd_ParseCommand_CMD_PAUSE();
    mtInterfaceUnlock(LOCKER_SERVICE);
    return 0;
}
//继续
MOTOIN9_DLL_API CALL int Motion_CommandResume() {
    DLog("Tro->Get Command->Resume");
    mtInterfaceLock(LOCKER_SERVICE, SERVICE_MUTEX_OUT_TIME);
    if (Controller_CheckInitialDone() != 0) {
        mtInterfaceUnlock(LOCKER_SERVICE);
        return -1;
    }
    if (Controller_CheckEmergency() != 0) {
        mtInterfaceUnlock(LOCKER_SERVICE);
        return -1;
    }
    int ret = Cmd_ParseCommand_CMD_RESUME();
    mtInterfaceUnlock(LOCKER_SERVICE);
    return ret;
}

//改变进给率
MOTOIN9_DLL_API CALL int Motion_CommandChangeGlobleVelocity(float feed_rate) {
    DLog("Tro->Get Command->ChangelVel,Vel=%f", feed_rate);
    mtInterfaceLock(LOCKER_SERVICE, SERVICE_MUTEX_OUT_TIME);
    if (Controller_CheckInitialDone() != 0) {
        mtInterfaceUnlock(LOCKER_SERVICE);
        return -1;
    }
    if (Controller_CheckEmergency() != 0) {
        mtInterfaceUnlock(LOCKER_SERVICE);
        return -1;
    }
    int ret = Cmd_ParseCommand_CMD_ChangeVel(feed_rate);
    mtInterfaceUnlock(LOCKER_SERVICE);
    return ret;
}

//************************************
// Method:    Motion_Add_Joint_Free_Jog_Increment_Linked
// FullName:  Motion_Add_Joint_Free_Jog_Increment_Linked
// Access:    public
// Returns:   MOTOIN9_DLL_API CALL int
// Qualifier: 参数在限定范围内
// Parameter: char blocking: 0->no blocking 1->blocking
// Parameter: double increment[MAX_JOINT]
// Parameter: STRUCT_USER_VEL user_vel
// Parameter: int id
// Parameter: char enable
//************************************
MOTOIN9_DLL_API CALL int Motion_Add_Joint_Free_Jog_Increment_Linked(char blocking, double increment[MAX_JOINT], STRUCT_USER_VEL user_vel, int id, char enable) {
    mtInterfaceLock(LOCKER_SERVICE, SERVICE_MUTEX_OUT_TIME);
    if (Controller_CheckInitialDone() != 0) {
        mtInterfaceUnlock(LOCKER_SERVICE);
        return -1;
    }
    if (Controller_CheckEmergency() != 0) {
        mtInterfaceUnlock(LOCKER_SERVICE);
        return -1;
    }
    if (Controller_CheckInterModeFree(id) != 0) {
        mtInterfaceUnlock(LOCKER_SERVICE);
        return -1;
    }
    if (Controller_CheckPause() != 0) {
        mtInterfaceUnlock(LOCKER_SERVICE);
        return -1;
    }
    if (Controller_CheckAbort() != 0) {
        mtInterfaceUnlock(LOCKER_SERVICE);
        return -1;
    }
    if (frAddJointIncrementLink(&Ct.fr, blocking, increment, user_vel, id, enable) != 0) {
        mtInterfaceUnlock(LOCKER_SERVICE);
        return -1;
    }
    mtInterfaceUnlock(LOCKER_SERVICE);
    return 0;
}

//FREE关节增量独立运动
MOTOIN9_DLL_API CALL int Motion_Add_Joint_Free_Jog_Increment_Independence(char blocking, int joint, double increment, STRUCT_USER_VEL user_vel, int id, char enable) {
    mtInterfaceLock(LOCKER_SERVICE, SERVICE_MUTEX_OUT_TIME);
    if (Controller_CheckInitialDone() != 0) {
        mtInterfaceUnlock(LOCKER_SERVICE);
        return -1;
    }
    if (Controller_CheckEmergency() != 0) {
        mtInterfaceUnlock(LOCKER_SERVICE);
        return -1;
    }
    if (Controller_CheckInterModeFree(id) != 0) {
        mtInterfaceUnlock(LOCKER_SERVICE);
        return -1;
    }
    if (Controller_CheckPause() != 0) {
        mtInterfaceUnlock(LOCKER_SERVICE);
        return -1;
    }
    if (Controller_CheckAbort() != 0) {
        mtInterfaceUnlock(LOCKER_SERVICE);
        return -1;
    }
    if (frAddJointIncrementIndep(&Ct.fr, blocking, joint, increment, user_vel, id, enable) != 0) {
        mtInterfaceUnlock(LOCKER_SERVICE);
        return -1;
    }
    mtInterfaceUnlock(LOCKER_SERVICE);
    return 0;
}

//FREE关节绝对联动
MOTOIN9_DLL_API CALL int Motion_Add_Joint_Free_Jog_Abstract_Linked(char blocking, double posit[MAX_JOINT], STRUCT_USER_VEL user_vel, int id, char enable) {
    mtInterfaceLock(LOCKER_SERVICE, SERVICE_MUTEX_OUT_TIME);
    if (Controller_CheckInitialDone() != 0) {
        mtInterfaceUnlock(LOCKER_SERVICE);
        return -1;
    }
    if (Controller_CheckEmergency() != 0) {
        mtInterfaceUnlock(LOCKER_SERVICE);
        return -1;
    }
    if (Controller_CheckInterModeFree(id) != 0) {
        mtInterfaceUnlock(LOCKER_SERVICE);
        return -1;
    }
    if (Controller_CheckPause() != 0) {
        mtInterfaceUnlock(LOCKER_SERVICE);
        return -1;
    }
    if (Controller_CheckAbort() != 0) {
        mtInterfaceUnlock(LOCKER_SERVICE);
        return -1;
    }
    if (frAddJointAbsLinked(&Ct.fr, blocking, posit, user_vel, id, enable) != 0) {
        mtInterfaceUnlock(LOCKER_SERVICE);
        return -1;
    }
    mtInterfaceUnlock(LOCKER_SERVICE);
    return 0;
}

//FREE关节绝对独立运动
MOTOIN9_DLL_API CALL int Motion_Add_Joint_Free_Jog_Abstract_Independence(char blocking, int joint, double posit, STRUCT_USER_VEL user_vel, int id, char enable) {
    mtInterfaceLock(LOCKER_SERVICE, SERVICE_MUTEX_OUT_TIME); //等待运算周期空闲
    if (Controller_CheckInitialDone() != 0) {
        mtInterfaceUnlock(LOCKER_SERVICE);
        return -1;
    }
    if (Controller_CheckEmergency() != 0) {
        mtInterfaceUnlock(LOCKER_SERVICE);
        return -1;
    }
    if (Controller_CheckInterModeFree(id) != 0) {
        mtInterfaceUnlock(LOCKER_SERVICE);
        return -1;
    }
    if (Controller_CheckPause() != 0) {
        mtInterfaceUnlock(LOCKER_SERVICE);
        return -1;
    }
    if (Controller_CheckAbort() != 0) {
        mtInterfaceUnlock(LOCKER_SERVICE);
        return -1;
    }
    if (frAddJointAbsIndep(&Ct.fr, blocking, joint, posit, user_vel, id, enable) != 0) {
        mtInterfaceUnlock(LOCKER_SERVICE);
        return -1;
    }
    mtInterfaceUnlock(LOCKER_SERVICE);
    return 0;
}

//FREE坐标系绝对位置运动
MOTOIN9_DLL_API CALL int Motion_Add_Axis_Free_Jog_Abstract(char blocking, STRUCT_POSE pose, int ik_flag, STRUCT_USER_VEL user_vel, int id, char enable) {
    mtInterfaceLock(LOCKER_SERVICE, SERVICE_MUTEX_OUT_TIME); //等待运算周期空闲
    if (Controller_CheckInitialDone() != 0) {
        mtInterfaceUnlock(LOCKER_SERVICE);
        return -1;
    }
    if (Controller_CheckEmergency() != 0) {
        mtInterfaceUnlock(LOCKER_SERVICE);
        return -1;
    }
    if (Controller_CheckInterModeFree(id) != 0) {
        mtInterfaceUnlock(LOCKER_SERVICE);
        return -1;
    }
    if (Controller_CheckPause() != 0) {
        mtInterfaceUnlock(LOCKER_SERVICE);
        return -1;
    }
    if (Controller_CheckAbort() != 0) {
        mtInterfaceUnlock(LOCKER_SERVICE);
        return -1;
    }
    if (frAddAxisAbs(&Ct.fr, blocking, pose, ik_flag, user_vel, id, enable) != 0) {
        mtInterfaceUnlock(LOCKER_SERVICE);
        return -1;
    }
    mtInterfaceUnlock(LOCKER_SERVICE);
    return 0;
}

//轨迹规划点到点位置参数
MOTOIN9_DLL_API CALL int Motion_AddPlaneP2P_Posit(double posit[MAX_JOINT], STRUCT_USER_VEL user_vel, STRUCT_USER_OVL user_ovl, int id, char enable) {
    mtInterfaceLock(LOCKER_SERVICE, SERVICE_MUTEX_OUT_TIME); //等待运算周期空闲
    if (Controller_CheckInitialDone() != 0) {
        mtInterfaceUnlock(LOCKER_SERVICE);
        return -1;
    }
    if (Controller_CheckEmergency() != 0) {
        mtInterfaceUnlock(LOCKER_SERVICE);
        return -1;
    }
    if (Controller_CheckInterModePlane(id) != 0) {
        mtInterfaceUnlock(LOCKER_SERVICE);
        return -1;
    }
    if (Controller_CheckPause() != 0) {
        mtInterfaceUnlock(LOCKER_SERVICE);
        return -1;
    }
    if (Controller_CheckAbort() != 0) {
        mtInterfaceUnlock(LOCKER_SERVICE);
        return -1;
    }
    if (Controller_CheckPlaneErrorOccur() != 0) {
        mtInterfaceUnlock(LOCKER_SERVICE);
        return -1;
    }
    if (tpAddP2P(posit, user_vel, user_ovl, id, enable) != 0) {
        mtInterfaceUnlock(LOCKER_SERVICE);
        return -1;
    }
    mtInterfaceUnlock(LOCKER_SERVICE);
    return 0;
}

//轨迹规划点到点位置参数
MOTOIN9_DLL_API CALL int Motion_AddPlaneP2P_Pose(STRUCT_POSE pose, int ik_flag, STRUCT_USER_VEL user_vel, STRUCT_USER_OVL user_ovl, int id, char enable) {
    mtInterfaceLock(LOCKER_SERVICE, SERVICE_MUTEX_OUT_TIME); //等待运算周期空闲
    if (Controller_CheckInitialDone() != 0) {
        mtInterfaceUnlock(LOCKER_SERVICE);
        return -1;
    }
    if (Controller_CheckEmergency() != 0) {
        mtInterfaceUnlock(LOCKER_SERVICE);
        return -1;
    }
    if (Controller_CheckInterModePlane(id) != 0) {
        mtInterfaceUnlock(LOCKER_SERVICE);
        return -1;
    }
    if (Controller_CheckPause() != 0) {
        mtInterfaceUnlock(LOCKER_SERVICE);
        return -1;
    }
    if (Controller_CheckAbort() != 0) {
        mtInterfaceUnlock(LOCKER_SERVICE);
        return -1;
    }
    if (Controller_CheckPlaneErrorOccur() != 0) {
        mtInterfaceUnlock(LOCKER_SERVICE);
        return -1;
    }
    if (tpAddP2P_Pose(pose, ik_flag, user_vel, user_ovl, id, enable)) {
        mtInterfaceUnlock(LOCKER_SERVICE);
        return -1;
    }
    mtInterfaceUnlock(LOCKER_SERVICE);
    return 0;
}

MOTOIN9_DLL_API CALL int Motion_AddPlaneLine_Pose(STRUCT_POSE pose, int ik_flag, STRUCT_USER_VEL user_vel, STRUCT_USER_OVL user_ovl, int id, char enable) {
    mtInterfaceLock(LOCKER_SERVICE, SERVICE_MUTEX_OUT_TIME); //等待运算周期空闲
    if (Controller_CheckInitialDone() != 0) {
        mtInterfaceUnlock(LOCKER_SERVICE);
        return -1;
    }
    if (Controller_CheckEmergency() != 0) {
        mtInterfaceUnlock(LOCKER_SERVICE);
        return -1;
    }
    if (Controller_CheckInterModePlane(id) != 0) {
        mtInterfaceUnlock(LOCKER_SERVICE);
        return -1;
    }
    if (Controller_CheckPause() != 0) {
        mtInterfaceUnlock(LOCKER_SERVICE);
        return -1;
    }
    if (Controller_CheckAbort() != 0) {
        mtInterfaceUnlock(LOCKER_SERVICE);
        return -1;
    }
    if (Controller_CheckPlaneErrorOccur() != 0) {
        mtInterfaceUnlock(LOCKER_SERVICE);
        return -1;
    }
    if (tpAddLine(pose, ik_flag, user_vel, user_ovl, id, enable)) {
        mtInterfaceUnlock(LOCKER_SERVICE);
        return -1;
    }
    mtInterfaceUnlock(LOCKER_SERVICE);
    return 0;
}

/*
----------------------query current state-------------------
*/
//运动完成
MOTOIN9_DLL_API CALL int Motion_QueryMoveDone() {
    if (Ct.running || jcqLen(&Ct.joint_control.jcq))
        return 1;
    else if (poqLen(&Ct.s_po.queue) != 0)
        return 1;
    else
        return 0;
}
//运动buff
MOTOIN9_DLL_API CALL int Motion_QueryPathBuffUsedSize() {
    return tcqLen(&Ct.tp.queue);
}
//Version
MOTOIN9_DLL_API CALL int Motion_QueryVersion(char *str) {
    memcpy(str, CTRL_VERSION, 16);
    return 0;
}
//初始化状态
MOTOIN9_DLL_API CALL int Motion_QueryInitial() {
    return Ct.init_done;
}
//指令状态
MOTOIN9_DLL_API CALL int Motion_QueryCmdState(ENUM_CMD_STATUS *State) {
    *State = Ct.cmd_status;
    return 0;
}
//运动模式
MOTOIN9_DLL_API CALL int Motion_QueryInterplationParten() {
    return Ct.inter_mode;
}
//查询配置
MOTOIN9_DLL_API CALL int Motion_QueryConfiguration(STRUCT_MOTION_PARM *conf) {
    memcpy(conf, &Ct.conf, sizeof(STRUCT_MOTION_PARM));
    return 0;
}
//查询全局速度
MOTOIN9_DLL_API CALL int Motion_QueryGlobleVelocity(float *vel) {
    *vel = Ct.feed_rate;
    return 0;
}
//查询执行的ID
MOTOIN9_DLL_API CALL int Motion_QueryExcuteId() {
    return Ct.execute_id;
}
//查询关节运动完成
MOTOIN9_DLL_API CALL int Motion_QueryJointMoveDone(int joint) {
    if (ckJointIndex(joint, -1) != 0) return -1;
    return Ct.joint_busy_flag[joint];
}
//查询当前theta值
MOTOIN9_DLL_API CALL int Motion_QueryCurrentPosition(double theta[MAX_JOINT]) {
    memcpy(theta, Ct.curr_posit, sizeof(double) * MAX_JOINT);
    return 0;
}

//查询当前位姿值
MOTOIN9_DLL_API CALL int Motion_QueryCurrentPose(STRUCT_POSE *pose, int *fk_flag) {
    int ret = Kinematic_ClampForward(Ct.curr_posit, pose, fk_flag);
    return ret;
}

//等待运动完成,joint = -1表示等待所有关节
MOTOIN9_DLL_API CALL int Motion_WaitMoveDone(int cycle, int times) {
    int cnt = 0, actual_cyc = 0;
    if (cycle < 5) {
        actual_cyc = 5;
    } else {
        actual_cyc = cycle;
    }
    if (times <= 0) {
        return 0;
    }
    while (Motion_QueryMoveDone() != 0) {
        C_SLEEP_MS(actual_cyc);
        cnt++;
        if (cnt > times) return -1;
    }
    return 0;
}

//************************************
// Method:    Motion_KmClampForward
// FullName:  Motion_KmClampForward
// Access:    运动学正解和逆解
// Returns:   MOTOIN9_DLL_API CALL int
// Qualifier:
// Parameter: double posit[MAX_JOINT]
// Parameter: STRUCT_POSE * pose
// Parameter: int * fk_flag
//************************************
MOTOIN9_DLL_API CALL int Motion_KmClampForward(double posit[MAX_JOINT], STRUCT_POSE *pose, int *fk_flag) {
    return Kinematic_ClampForward(posit, pose, fk_flag);
}

MOTOIN9_DLL_API CALL int Motion_KmClampInverse(double posit[MAX_JOINT], STRUCT_POSE *pose, int ik_flag) {
    return Kinematic_ClampInverse(posit, pose, ik_flag, NULL);
}

MOTOIN9_DLL_API CALL int Motion_KmHandInverse(double posit[MAX_JOINT], STRUCT_POSE *pose, int ik_flag) {
    return Kinematic_HandInverse(posit, pose, ik_flag, NULL);
}

MOTOIN9_DLL_API CALL int Motion_KmHandForward(double posit[MAX_JOINT], STRUCT_POSE *pose, int *fk_flag) {
    return Kinematic_HandForward(posit, pose, fk_flag);
}

/*
----------------运动控制service函数-------------------
*/
MOTOIN9_DLL_API CALL void Motion_Service() {
    struct timeval tvs, tve;
    gettimeofday(&tvs, NULL);
    mtInterfaceLock(LOCKER_SERVICE, SERVICE_MUTEX_OUT_TIME);
    Controller_RunCycle();
    mtInterfaceUnlock(LOCKER_SERVICE);
    gettimeofday(&tve, NULL);
    double span = 2000.0-((tve.tv_sec - tvs.tv_sec)*1000000.0 + (tve.tv_usec - tvs.tv_usec));
    if(span > 0 && span < 2000){
        usleep(span);
    }
    return;
}

MOTOIN9_DLL_API CALL void Motion_Out() {
    //mtInterfaceLock(LOCKER_CMD, SERVICE_MUTEX_OUT_TIME);
    STRUCT_PO_SEGMENT *po_segment;
    po_segment = poqItem(&Ct.s_po.queue, 0);
    if (po_segment != NULL) {
        Ct.conf.callback_out(po_segment->posit, po_segment->vel, po_segment->acc);
        poqRemove(&Ct.s_po.queue, 1);
    }
    //mtInterfaceUnlock(LOCKER_CMD);
    return;
}
